perm filename TRAJ.SAI[SYS,HE]8 blob
sn#070332 filedate 1973-11-05 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00022 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00004 00002 DEFINE WRITE="DEB_HAND"
C00008 00003 SIMPLE PROCEDURE COMPLETA (INTEGER ISAFE REAL ARRAY TH)
C00010 00004 SIMPLE PROCEDURE INCREMENT(SAFE REAL ARRAY DTH,DX_DY_DZBOOLEAN COMP)
C00014 00005 SIMPLE PROCEDURE FORCE(SAFE REAL ARRAY TQ,FV)
C00018 00006 SIMPLE PROCEDURE SCHEINMAN (SAFE REAL ARRAY RES,TH)
C00021 00007 SIMPLE PROCEDURE FLUSH(BOOLEAN FINISHSAFE REAL ARRAY TS)
C00024 00008 SIMPLE PROCEDURE REVOLVE(SAFE REAL ARRAY P,O REAL TH)
C00026 00009 EXTERNAL SIMPLE PROCEDURE PACK(REFERENCE INTEGER PTRINTEGER PERIODREFERENCE REAL BUF)
C00031 00010 IFC WAVE THENC
C00035 00011 BOOLEAN SIMPLE PROCEDURE OVERSHOOT(REAL A3,A4,DELTA,OFFREFERENCE REAL T,D)
C00039 00012 SIMPLE MESSAGE PROCEDURE TRAJECTORY(REAL ARRAY T0,TF)
C00042 00013 SIMPLE MESSAGE PROCEDURE MOVE_ARM(REAL ARRAY TREFERENCE INTEGER FLAG)
C00045 00014 SIMPLE MESSAGE PROCEDURE GO_ARM(REAL ARRAY TREFERENCE INTEGER FLAG)
C00049 00015 SIMPLE MESSAGE PROCEDURE DRAW_ARM(REAL ARRAY PROFILEINTEGER STAT)
C00054 00016 SIMPLE MESSAGE PROCEDURE START_TRAJECTORY(STRING FILEINTEGER START)
C00056 00017 SIMPLE MESSAGE PROCEDURE OPEN_HAND(REAL OPENING)
C00061 00018 SIMPLE MESSAGE PROCEDURE CHANGE_ARM(REAL ARRAY DIRREAL DIST
C00064 00019 SIMPLE MESSAGE PROCEDURE CENTER_HAND(REAL DIST)
C00066 00020 SIMPLE MESSAGE PROCEDURE DRIVE_ARM(INTEGER JOINTREAL DEGINTEGER TIMEREFERENCE INTEGER FLAG)
C00070 00021 SIMPLE MESSAGE PROCEDURE DO_IT(STRING FILE)
C00073 00022 SIMPLE MESSAGE PROCEDURE ARM_SAVE(STRING S)
C00078 ENDMK
C⊗;
DEFINE WRITE="DEB_HAND";
REQUIRE "VECTOR.SAI[SYS,HE]" SOURCE_FILE;
EXTERNAL INTEGER _SKIP_;
DEFINE AVT (A, S, CA, SA)="1.0, -CA, SA, A, 1.0, CA, -SA, A, 0.0, SA, CA, S, [3] 0.0, 1.0, 0.0 ";
DEFINE AVP (A, S, CA, SA)="-CA, SA, A, CA, -SA, A";
DEFINE AVS (A, CA, SA, CT, ST)="CT, -CA*ST, SA*ST, A*CT, ST, CA*CT, -SA*CT, A*ST,
0.0, SA, CA, 1.0, [3] 0.0, 1.0, 0.0 ";
DEFINE QT = "0.0, -1.0, 0.0, 0.0, 1.0, [12] 0.0 ";
DEFINE QS = "[11] 0.0, 1.0, [5] 0.0 ";
DEFINE JDEF (M, X, Y, Z, IXX, IYY, IZZ) =".5*(-IXX+IYY+IZZ), 0.0, 0.0, M*X, 0.0,
.5*(IXX-IYY+IZZ), 0.0, M*Y, 0.0, 0.0, .5*(IXX+IYY-IZZ), M*Z, M*X, M*Y, M*Z, M, 0.0 ";
REQUIRE "YELLOW.SAI" SOURCE_FILE;
SAFE REAL ARRAY U1,T[0:101];
SAFE REAL ARRAY DIAG[1:7];
SAFE REAL ARRAY LAST_PLANNED_TRANS,PARK_TRANS[1:4,1:4];
SAFE REAL ARRAY LAST_PLANNED_ARM[1:6];
SAFE INTEGER ARRAY COEFF[0:'1037];
SAFE REAL ARRAY ARRIVE_ARM,DEPART_ARM[1:4];
REAL F1_ARM,F2_ARM,OBJECT_MASS,OBJECT_KXX,OBJECT_KYY,OBJECT_KZZ;
INTEGER T1_ARM,T2_ARM,MERGE;
INTEGER BFJ;
DEFINE MAX_SEG="8";
SAFE REAL ARRAY POSITION[1:6,0:MAX_SEG];
SAFE REAL ARRAY KOE[1:6,1:(MAX_SEG*3)+6];
SAFE INTEGER ARRAY PERIOD[1:6,1:MAX_SEG];
SAFE INTEGER ARRAY NS[1:6];
SAFE REAL ARRAY POS[0:MAX_SEG];
SAFE REAL ARRAY SOLN[1:(MAX_SEG*3)+6];
SAFE INTEGER ARRAY TIM[1:MAX_SEG];
SAFE STRING ARRAY BANDS[0:'37];
SAFE INTEGER ARRAY TRACK[0:'40];
INTEGER NEXT_BAND,BAND,BAND_NUMBER;
PRELOAD_WITH 0,'1040,0;
SAFE INTEGER ARRAY GOODIE[1:3];
DEFINE UFBWRT="'707000000000";
BOOLEAN FORCED,WAS_FORCED,FAST;
SAFE REAL ARRAY LAST_ARM[1:6];
SAFE REAL ARRAY LAST_TRANS[1:4,1:4];
SAFE REAL ARRAY QA,D[0:16];
SAFE REAL ARRAY TF[1:6];
SAFE REAL ARRAY LU,NR[1:6,1:6];
REAL DIGITS;
INTEGER LOOP,JOINT,DURATION;
SAFE REAL ARRAY REAL_DATA[1:18];
REQUIRE "MATRIX.REL[SYS,HE]" LOAD_MODULE;
REQUIRE "SAILIB.REL[SYS,HE]" LOAD_MODULE;
EXTERNAL SIMPLE PROCEDURE MMOVE(REFERENCE REAL R,A);
EXTERNAL SIMPLE PROCEDURE MTIMES(REFERENCE REAL R,A,B);
EXTERNAL REAL SIMPLE PROCEDURE TRACET(REFERENCE REAL X,Y);
EXTERNAL SIMPLE PROCEDURE DECOMPOSE(INTEGER I; REAL ARRAY NR,LU);
EXTERNAL SIMPLE PROCEDURE SOLVE(INTEGER I;REAL ARRAY NR,LU,DTH);
EXTERNAL SIMPLE PROCEDURE IMPROVE(INTEGER I; REAL ARRAY NR,LU,DA,DTH;REFERENCE REAL DIGITS);
INTEGER SIMPLE PROCEDURE SQAR (INTEGER I);
RETURN ((I-1)*17);
SIMPLE PROCEDURE COMPLETA (INTEGER I;SAFE REAL ARRAY TH);
BEGIN REAL S,C;
INTEGER J,K;
J←SQAR(I);
K←6*(I-1);
S←SIND(TH[I]);
C←COSD(TH[I]);
A[J]←C;
A[J+1]←APAR[K]*S;
A[J+2]←APAR[K+1]*S;
A[J+3]←APAR[K+2]*C;
A[J+4]←S;
A[J+5]←APAR[K+3]*C;
A[J+6]←APAR[K+4]*C;
A[J+7]←APAR[K+5]*S;
END;
SIMPLE PROCEDURE HANDPOS(SAFE REAL ARRAY TH);
BEGIN INTEGER I;
FOR I←1,2,4,5,6 DO COMPLETA(I,TH);
A[SQAR(3)+11]←TH[3];
A[3]←A[3]+SHOLDER[1];
A[7]←A[7]+SHOLDER[2];
MMOVE(A[0],A[0]);
MMOVE(T[0],A[0]);
UNDERFLOW(-1);
FOR I←2 STEP 1 UNTIL 6 DO MTIMES(T[SQAR(I)],T[SQAR(I-1)],A[SQAR(I)]);
UNDERFLOW(0);
END;
SIMPLE PROCEDURE UPDATE_SEG;
BEGIN INTEGER I;
HANDPOS(ARM_VECTOR);
FOR I←3 STEP 1 UNTIL 6 DO
BEGIN
ARRBLT(ARM_LINK[I,1,1],T[SQAR(I)],16);
END;
GRASP←ARM_VECTOR[7];
END;
SIMPLE PROCEDURE NOT_LESS(REFERENCE REAL V;REAL L);
BEGIN
IF V<0.0 ∧ V>-L THEN BEGIN V←0; RETURN END;
IF V≥0.0 ∧ V< L THEN V←0;
END;
SIMPLE PROCEDURE INCREMENT(SAFE REAL ARRAY DTH,DX_DY_DZ;BOOLEAN COMP);
BEGIN PRELOAD_WITH 3,7,11;
SAFE OWN INTEGER ARRAY KEY[1:6];
PRELOAD_WITH [6] 0.0; SAFE OWN REAL ARRAY DIR[1:6];
INTEGER I,J;
REAL MAX,R;
LABEL OUT;
REDUCE(DX_DY_DZ);
IF DX_DY_DZ[1]=0.0 ∧
DX_DY_DZ[2]=0.0 ∧
DX_DY_DZ[3]=0.0 THEN BEGIN FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←0.0;
RETURN;
END;
UNDERFLOW(-1);
ARRBLT(DIR[1],DX_DY_DZ[1],3);
IF ¬COMP THEN BEGIN
IF ABS(T[85+2])<ABS(T[85+6])
THEN BEGIN KEY[4]←2;
KEY[5]←IF ABS(T[85+6])<ABS(T[85+10]) THEN 6 ELSE 10
END
ELSE BEGIN KEY[4]←6;
KEY[5]←IF ABS(T[85+2])<ABS(T[85+10]) THEN 2 ELSE 10
END;
KEY[6]←IF ABS(T[85+4])>ABS(T[85+0])
THEN IF ABS(T[85+8])>ABS(T[85+4]) THEN 9 ELSE 5
ELSE IF ABS(T[85+8])>ABS(T[85+0]) THEN 9 ELSE 1;
MMOVE(U1[SQAR(6)],A[SQAR(6)]);
FOR I←5 STEP -1 UNTIL 2 DO
BEGIN MTIMES(U1[SQAR(I)],A[SQAR(I)],U1[SQAR(I+1)]);
MTIMES(U1[SQAR(I+1)],Q[(IF I=2 THEN 17 ELSE 0)],U1[SQAR(I+1)])
END;
A[3]←A[7]←0.0;
MMOVE(A[0],A[0]);
MTIMES(U1[0],A[0],U1[SQAR(2)]);
MTIMES(U1[SQAR(2)],Q[0],U1[SQAR(2)]);
FOR I←2 STEP 1 UNTIL 6 DO MTIMES(U1[SQAR(I)],T[SQAR(I-1)],U1[SQAR(I)]);
MTIMES(U1[0],Q[0],U1[0]);
FOR I←1 STEP 1 UNTIL 6 DO
FOR J←1 STEP 1 UNTIL 6 DO
NR[J,I]←U1[SQAR(I)+KEY[J]];
DECOMPOSE(6,NR,LU);
END;
SOLVE(6,LU,DIR,DTH);
IMPROVE(6,NR,LU,DIR,DTH,DIGITS);
MAX←0;
FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DIR[I]))>MAX THEN MAX←R;
FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DTH[I]))>MAX THEN
BEGIN FOR J←I+1 STEP 1 UNTIL 6 DO IF ABS(DTH[I]+DTH[J])/R<0.5 THEN
BEGIN DTH[I]←0;
DTH[J]←0;
GO TO OUT;
END;
END;
OUT: FOR I←1 STEP 1 UNTIL 6 DO BEGIN
DTH[I]←IF I=3 THEN DTH[I] ELSE RAD*DTH[I];
NOT_LESS(DTH[I],@-5);
END;
UNDERFLOW(0);
END;
INTEGER SIMPLE PROCEDURE LIMITS(SAFE REAL ARRAY JOINT);
BEGIN INTEGER I;
FOR I←1 STEP 1 UNTIL 5 DO
IF (STOP[I,1]-JOINT[I])*(JOINT[I]-STOP[I,2])<0.0 THEN RETURN(I);
RETURN (0);
END;
SIMPLE PROCEDURE FAST_WRITE;
IF FAST
THEN BEGIN LABEL OK;
START_CODE LABEL WRT;
HRRZ 1,GOODIE;
HRRZ 2,COEFF;
MOVEM 2,(1);
MOVE 3,BAND_NUMBER;
MOVEI 4,10;
WRT: UFBWRT 3,(1);
SOJGE 4,WRT;
MOVEI 2,'20;
ADDM 2,2(1);
JUMPGE 4,OK;
END;
USERERR(0,1,"TRAJECTORY WRITE ERROR");
OK: END
ELSE ARRYOUT('16,COEFF[0],'1000);
SIMPLE PROCEDURE FORCE(SAFE REAL ARRAY TQ,FV);
BEGIN
STRING S;
INTEGER I,L;
REAL XS,YS;
SAFE OWN REAL ARRAY F,M,P[1:4];
SAFE OWN REAL ARRAY INV[1:4,1:4];
EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY R;REFERENCE REAL A;REAL ARRAY V);
EXTERNAL SIMPLE PROCEDURE INVERT(REFERENCE REAL R,A);
SIMPLE PROCEDURE COLVECT(REAL ARRAY R;INTEGER L,I);
BEGIN INTEGER K;
FOR K←1 STEP 1 UNTIL 3 DO R[K]←A[L+(K-1)*4+(I-1)];
R[4]←1.0;
END;
PUSH_FORMAT(9,1);
ARRBLT(F[1],FV[1],3);
ARRBLT(M[1],FV[4],3);
INVERT(INV[1,1],T[SQAR(6)]);
F[4]←M[4]←0.0;
TRANSFORM(F,INV[1,1],F);
TRANSFORM(M,INV[1,1],M);
A[3]←A[7]←0.0;
FOR L←6 STEP -1 UNTIL 1 DO
BEGIN
F[4]←M[4]←0.0;
TRANSFORM (F,A[SQAR(L)],F);
COLVECT(P,SQAR(L),4);
F[4]←1.0;
CROSS(P,P,F);
TRANSFORM(M,A[SQAR(L)],M);
M[4]←1.0;
PLUS(M,M,P);
REDUCE(M);
TQ[L]←IF L=3 THEN F[3] ELSE M[3];
END;
A[3]←A[3]+SHOLDER[1];
A[7]←A[7]+SHOLDER[2];
S←"JOINT TORQUES";
FOR I←1 STEP 1 UNTIL 6 DO S←S&(CVF(TQ[I]));
IF TYP_HAND THEN OUTSTR(S&CRLF&CRLF);
POP_FORMAT;
END;
PRELOAD_WITH
'400000200000,
'200000040000,
'100000010000,
'40000002000,
'20000000400,
'10000000100;
SAFE OWN INTEGER ARRAY GO_WORD[1:6];
SIMPLE PROCEDURE BITS(REFERENCE INTEGER DATWD;REAL SIGN;REFERENCE INTEGER DAT);
START_CODE
MOVE 3,-3('17);
MOVE 1,(3);
MOVE 2,@-1('17);
TDO 1,2;
TRZ 1,(2);
LSH 2,1;
SKIPGE -2('17);
TRO 1,(2);
MOVEM 1,(3);
END;
PROCEDURE FREE_JOINT(REFERENCE INTEGER DATWD);
BEGIN INTEGER NF,I,J,K;
REAL R,MAX;
SAFE OWN INTEGER ARRAY ZF[1:6];
SAFE OWN REAL ARRAY FV,TQ[1:6];
NF←FREE_ARM[0,1];
FOR I←1 STEP 1 UNTIL NF DO
BEGIN
ARRBLT(FV[1],FREE_ARM[I,1],6);
FORCE(TQ,FV);
MAX←0.0;
FOR J←1 STEP 1 UNTIL 6 DO
BEGIN LABEL L1;
IF J=BFJ THEN GO TO L1;
FOR K←1 STEP 1 UNTIL I-1 DO IF ZF[K]=J THEN GO TO L1;
IF (R←ABS(TQ[J]/F0[J]))>MAX THEN
BEGIN
MAX←R;
ZF[I]←J;
END;
L1: END;
BITS(DATWD,TQ[ZF[I]],GO_WORD[ZF[I]]);
END;
END;
SIMPLE INTEGER PROCEDURE RUNTIME(SAFE REAL ARRAY DTH);
BEGIN
INTEGER T,TIME,I;
TIME←0;
FOR I←1 STEP 1 UNTIL 6 DO BEGIN
T←ABS(DTH[I])*TIMFAC[I];
IF T>TIME THEN TIME←T END;
RETURN(TIME+20);
END "RUNTIME";
SIMPLE PROCEDURE SCHEINMAN (SAFE REAL ARRAY RES,TH);
BEGIN
LABEL F2;
REAL M,IXX,IYY,IZZ;
SAFE OWN REAL ARRAY JI,TQ[1:6];
EXTERNAL REAL SIMPLE PROCEDURE INNER(REFERENCE REAL A,B);
REAL Z; INTEGER I,DIFF,J,K,L,TJ;
RES[1]←0;ARRBLT(RES[2],RES[1],6);
JI[1]←0;ARRBLT(JI[2],JI[1],5);
WAS_FORCED←FORCED;
FORCED←FALSE;
FORCED←OBJECT_MASS;
M←9.33*OBJECT_MASS+M6;
Z←(M6*Z6)/M;
IXX←I6XX+9.33*OBJECT_MASS*OBJECT_KXX↑2;
IYY←I6YY+9.33*OBJECT_MASS*OBJECT_KYY↑2;
IZZ←I6ZZ+9.33*OBJECT_MASS*OBJECT_KZZ↑2;
I←SQAR(6);
JMAT[I]←0.5*(-IXX+IYY+IZZ);
JMAT[I+5]←0.5*(IXX-IYY+IZZ);
JMAT[I+10]←0.5*(IXX+IYY-IZZ);
JMAT[I+11]←JMAT[I+14]←M*Z;
JMAT[I+15]←M;
COMPLETA(1,TH);
COMPLETA(2,TH);
A[SQAR(3)+11]←TH[3];
COMPLETA(4,TH);
COMPLETA(5,TH);
COMPLETA(6,TH);
UNDERFLOW(-1);
MTIMES(U1[0],Q[0],A[0]);
MTIMES(D[0],U1[0],JMAT[0]);
JI[1]←TRACET(D[0],U1[0]);
MMOVE(T[0],A[0]);
for J ← 2 step 1 until 6 do begin
TJ←SQAR(J);
DIFF←IF J=3 THEN 17 ELSE 0;
MTIMES (QA[0], Q[DIFF], A[TJ]);
MTIMES (U1[TJ], T[SQAR(J-1)], QA[0]);
for I← 1 step 1 until J-1 do MTIMES (U1[SQAR(I)],U1[SQAR(I)], A[TJ]);
MTIMES(T[TJ],T[SQAR(J-1)],A[TJ]);
F2: FOR I←1 STEP 1 UNTIL J DO RES[I]←RES[I]
+0.107*INNER(U1[SQAR(I)+8],JMAT[TJ+12]);
For K← 1 step 1 until J do begin
MTIMES (D[0],U1[SQAR(K)],JMAT[TJ]);
JI[K]←JI[K] + TRACET(D[0],U1[SQAR(K)]);
END;
END;
T[88]←T[88]+SX;T[92]←T[92]+SY;
FOR I←1 STEP 1 UNTIL 6 DO IF FORCE_ARM[I] THEN BEGIN
FORCE(TQ,FORCE_ARM);
FORCED←TRUE;
FOR I←1 STEP 1 UNTIL 6 DO RES[I]←RES[I]+TQ[I];
DONE;
END;
FOR I←1 STEP 1 UNTIL 6 DO NOT_LESS(RES[I],1.0@-5);
FOR I←1 STEP 1 UNTIL 6 DO RES[I]←(RES[I] LAND '777777000000) LOR (SQRT(JI[I]) LSH -18);
I←'770000252502;
IF NNUL THEN I←I+'1000000;
IF ¬(FORCED ∨ WAS_FORCED) THEN I←I+'2000000;
FREE_JOINT(I);
ARRBLT(RES[7],I,1);
UNDERFLOW(0);
END;
SIMPLE PROCEDURE FLUSH(BOOLEAN FINISH;SAFE REAL ARRAY TS);
BEGIN INTEGER I;
IF ¬FRST_OPEN ∨ FINISH
THEN BEGIN
IFC WAVE THENC
FOR I←1 STEP 1 UNTIL FREEL
DO FOR J←1 STEP 1 UNTIL PTR3+1
DO IF EQU(REF[J],LABELS[I])
THEN BEGIN
START_CODE
MOVE 1,STACK;
ADD 1,J;
HRRE 1,-1(1);
MOVEM 1,N END;
N←PTRS[I]-J+N;
REF[J]←NULL;
IF N+J<1 ∨ N+J>PTR3+1
THEN BEGIN
OUTSTR(LABEL_LINE[I]&LABELS[I]&" OUT OF RANGE"&'15&'12);
N←PTR3+1-J END;
STACK[J]←(N LAND '777777) LOR (STACK[J] LAND '777000000);
LABEL_LINE[I]←NULL END;
FOR I←1 STEP 1 UNTIL PTR3
DO IF LENGTH(REF[I])
THEN BEGIN OUTSTR(CODE_LINE[I]&REF[I]&" UNDEFINED"&'15&'12);
STACK[I]←(PTR3+1-I) LOR '102000000;
REF[I]←NULL;
LABEL_LINE[I]←NULL END;
FREEL←0;
FOR I←1 STEP 1 UNTIL PTR3 DO
IF (J←(STACK[I] LAND '777000000))='102000000 ∨ J='16000000 THEN BEGIN
START_CODE
MOVE 1,STACK;
ADD 1,I;
HRRE 1,-1(1);
MOVEM 1,N END;
IF ¬N ∨ N+I<1 ∨ N+I>PTR3+1 THEN BEGIN
OUTSTR(CODE_LINE[I]&"JUMP OUT OF RANGE"&CRLF);
STACK[I]←(PTR3+1-I) LOR '102000000 END;
END;
FOR I←1 STEP 1 UNTIL MAC DO BBEG[I]←LLAB[I]←1;
ENDC
IF PTR3+PTR4≥PTR2 THEN USERERR(0,1,"TRAJECTORY FILE TOO LONG");
ARRBLT(COEFF[PTR4+1],STACK[1],PTR3);
COEFF[PTR4+PTR3+1]←0;
I←-(PTR4+1);
ARRBLT(COEFF[0],I,1);
COEFF['1000]←0;
PTR4←PTR3←0;
PTR2←512;
FAST_WRITE;
END ELSE FRST_OPEN←FALSE;
IF FINISH THEN BEGIN
IF ¬FAST THEN RELEASE('16);
RETURN END;
ARRTRAN(TH,TS);
SCHEINMAN(DIAG,TH);
END;
SIMPLE PROCEDURE FLUSHP(INTEGER N;SAFE REAL ARRAY TH);
BEGIN FRST_OPEN←FALSE;
IF PTR3+PTR4+N≥511 THEN FLUSH(0,TH);
END;
SIMPLE PROCEDURE REVOLVE(SAFE REAL ARRAY P,O; REAL TH);
BEGIN
SAFE OWN REAL ARRAY OP,A,T[1:4];
UNIT(O,O);
SCALE(OP,O,DOT(P,O));
DIFFERENCE(A,P,OP);
CROSS(T,O,A);
SCALE(T,T,SIND(TH));
SCALE(P,A,COSD(TH));
PLUS(P,P,T);
PLUS(P,P,OP);
REDUCE(P);
END;
PROCEDURE RESET_CONO;
BEGIN
FREE_ARM[0,1]←FORCE_ARM[1]←OBJECT_MASS←OBJECT_KXX←OBJECT_KYY←OBJECT_KZZ←0;
NNUL←0;
BFJ←0;
ARRBLT(FORCE_ARM[2],FORCE_ARM[1],5);
END;
SIMPLE PROCEDURE LIFTOFF(SAFE REAL ARRAY TH,DTH,DIR;REFERENCE INTEGER N,T1);
BEGIN INTEGER I,J;
REAL F;
LABEL L2;
SAFE OWN REAL ARRAY TT[1:6];
F←1.0;
IF LIMITS(TH) THEN BEGIN
FOR I←1 STEP 1 UNTIL 5 DO DTH[I]←
IF TH[I]≥STOP[I,2] THEN STOP[I,2]-TH[I]
ELSE IF TH[I]≤STOP[I,1] THEN STOP[I,1]-TH[I]
ELSE 0;
GO TO L2;
END;
HANDPOS(TH);
INCREMENT(DTH,DIR,FALSE);
FOR I←1 STEP 1 UNTIL 6 DO TT[I]←TH[I]+DTH[I];
WHILE I←LIMITS(TT) DO
BEGIN FOR J←1,2 DO
IF (TT[I]-STOP[I,J])*(STOP[I,J]-TH[I])≥0
THEN
BEGIN F←0.4*(STOP[I,J]-TH[I])/(TT[I]-TH[I]);
DTH[I]←DTH[I]*F;
TT[I]←TH[I]+DTH[I];
DONE;
END;
END;
L2: N←IF T1>0 THEN T1 ELSE RUNTIME(DTH);
END;
EXTERNAL SIMPLE PROCEDURE PACK(REFERENCE INTEGER PTR;INTEGER PERIOD;REFERENCE REAL BUF);
SIMPLE PROCEDURE PACK_UP;BEGIN
INTEGER P1,I,J,K;
REAL R1,R2;
SAFE OWN INTEGER ARRAY LOOPP,NXT,ALT[1:6];
SAFE OWN REAL ARRAY BUF[0:4];
COEFF[PTR4←PTR4+1]←DURATION;
STACK[PTR3←PTR3+1]←'21000000 LOR PTR4;
IF MERGE THEN BEGIN
STACK[PTR3]←STACK[PTR3] LOR '400000000;
STACK[PTR3]↔STACK[PTR3-1];
MERGE←FALSE END;
BUF[1]←0;BUF[2]←0;
FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
BUF[0]←POSITION[JOINT,0];
BUF[3]←KOE[JOINT,1];BUF[4]←KOE[JOINT,2];
PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,1],BUF[0]);
NXT[JOINT]←PTR4 END;
COEFF[PTR4←PTR4+1]←PTR2;
FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
BUF[4]←0;
K←IF LOOP THEN 7 ELSE NS[JOINT]-1;
FOR I←2 STEP 1 UNTIL K DO BEGIN
BUF[0]←POSITION[JOINT,I-1];
FOR J←1 STEP 1 UNTIL 3 DO BUF[J]←KOE[JOINT,(3*I)-4+J];
PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,I],BUF[0]);
COEFF[NXT[JOINT]]←(PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
NXT[JOINT]←PTR4;
IF LOOP THEN BEGIN
IF I=3 THEN LOOPP[JOINT]←PTR4;
IF I=6 THEN ALT[JOINT]←PTR4 END;
IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←PTR2←PTR2-7 END;
BUF[0]←POSITION[JOINT,I-1];
BUF[1]←-4.0*(R1←KOE[JOINT,(3*I)-2])+3.0*(R2←KOE[JOINT,(3*I)-3]);
BUF[2]← 6.0*R1-3.0*R2;
BUF[3]←-4.0*R1 +R2;
BUF[4]← R1;
PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,I],BUF[0]);
COEFF[NXT[JOINT]]←(PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
BUF[4]←0;
IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←PTR2←PTR2-7;
IF LOOP THEN BEGIN
BUF[0]←POSITION[JOINT,6];
FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I-2+J];
PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,3],BUF[0]);
NXT[JOINT]←PTR4;
COEFF[P1←ALT[JOINT]]←(PTR4 LSH 27) LOR COEFF[P1];
IF JOINT=6 THEN COEFF[P1+1]←'100000000000 LOR (LOOP LSH 18) LOR COEFF[P1+1];
IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←499;
BUF[0]←POSITION[JOINT,0];
FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I+2+J];
PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,3],BUF[0]);
COEFF[NXT[JOINT]]←(PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
COEFF[PTR4]←(LOOPP[JOINT] LSH 18) LOR COEFF[PTR4];
IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←473 END END;
END;
INTEGER SIMPLE PROCEDURE QUADROOT(REAL A0,A1,A2,A3;REFERENCE REAL ANG,T);BEGIN
REAL DISC,B;
INTEGER I;
IF A3 ∧ (DISC←(A2/(3*A3))↑2-A1/(3*A3))≥0 THEN DISC←SQRT(DISC) ELSE RETURN(0);
B←A2/(3*A3);
T←DISC-B;
FOR I←0,1 DO BEGIN
IF 0< T <1.0 THEN
BEGIN
ANG←(((A3*T)+A2)*T+A1)*T+A0;
IF ANG>STOP[JOINT,2] THEN BEGIN ANG←STOP[JOINT,2];RETURN(1) END;
IF ANG<STOP[JOINT,1] THEN BEGIN ANG←STOP[JOINT,1];RETURN(1) END;
END;
T←-(DISC+B);
END;
RETURN(0);
END;
IFC WAVE THENC
SIMPLE PROCEDURE STEP_ARM(INTEGER JOINT;REAL DTH;INTEGER TIME);
BEGIN
INTEGER I;
IF ARM_EXECUTE THEN RETURN;
FLUSHP(100,LAST_ARM);
FOR I←1 STEP 1 UNTIL 6 DO
BEGIN
POSITION[I,0]←LAST_ARM[I];
NS[I]←2;
PERIOD[I,1]←10;
PERIOD[I,2]←IF TIME>60 THEN TIME-10 ELSE 50;
IF I=JOINT THEN POSITION[I,1]←POSITION[I,2]←LAST_ARM[I]←LAST_ARM[I]+DTH
ELSE POSITION[I,1]←POSITION[I,2]←LAST_ARM[I];
END;
SCHEINMAN(DIAG,LAST_ARM);
ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
COEFF[PTR2+6]←GO_WORD[JOINT]+2;
IF NNUL THEN COEFF[PTR2+6]←COEFF[PTR2+6]+'1000000;
ARRBLT(COEFF[PTR2-7],COEFF[PTR2],7);
KOE[1,1]←0;
ARRBLT(KOE[1,2],KOE[1,1],6*((MAX_SEG*3)+6)-1);
PACK_UP;
DURATION←10;
END;
ENDC
PROCEDURE POLY(INTEGER N);BEGIN
SAFE REAL ARRAY A,LU[1:(3*MAX_SEG)+6,1:(3*MAX_SEG)+6];
SAFE REAL ARRAY B[1:(3*MAX_SEG)+6];
INTEGER I,J,K,P1,N3;
REAL T,T2;
SIMPLE PROCEDURE THREE(INTEGER I,J;REAL T);
BEGIN
FOR K←J STEP 1 UNTIL J+2 DO A[I+2,K]←1.0;
A[I,J]←-T;
A[I+1,J+1]←-(A[I+4,J+1]←2.0*(T2←T*T));
A[I+4,J+2]←6.0*T2;
A[I+3,J+2]←3.0*T;
A[I+3,J+1]←2.0*T;
A[I+3,J]←T;
END;
SIMPLE PROCEDURE FOUR(INTEGER I,J;REAL T);
BEGIN
FOR K←J STEP 1 UNTIL J+3 DO A[I+2,K]←1.0;
A[I,J]←-T;
A[I+1,J+1]←-(A[I+4,J+1]←2.0*(T2←T*T));
A[I+4,J+2]←6.0*T2;
A[I+4,J+3]←12.0*T2;
A[I+3,J+3]←4.0*T;
A[I+3,J+2]←3.0*T;
A[I+3,J+1]←2.0*T;
A[I+3,J]←T;
END;
UNDERFLOW(-1);
N3←N*3;
T←1.0/TIM[1];
T2←T*T;
A[1,1]←A[1,2]←1.0;
A[2,1]←3.0*T;
A[2,2]←4.0*T;
A[3,1]←6.0*T2;
A[3,2]←12.0*T2;
FOR J←1 STEP 1 UNTIL N-2 DO
THREE((3*J)-1,(3*J),1.0/TIM[J+1]);
A[N3-4,N3-3]←-3.0*(T←1.0/TIM[N]);
A[N3-4,N3-2]←4.0*T;
A[N3-3,N3-3]←6.0*(T2←T*T);
A[N3-3,N3-2]←-12.0*T2;
A[N3-2,N3-3]←1.0;
A[N3-2,N3-2]←-1.0;
FOR J←1 STEP 1 UNTIL N DO
B[(J*3)-2]←POS[J]-POS[J-1];
P1←N3-2;
IF LOOP THEN BEGIN
FOUR(N3-1,N3-1,(T←1/TIM[N-2]));
FOUR(N3+2,N3+3,T);
FOR J←6,7,8 DO BEGIN
A[N3+5,J]←A[5,J];
A[N3+6,J]←A[6,J] END;
FOR J←N3-9 STEP 1 UNTIL N3-7 DO BEGIN
A[N3-1,J]←A[N3-7,J];
A[N3,J]←A[N3-6,J] END;
B[N3+1]←B[N3-2]+B[N3-5];
B[N3+4]←B[1]+B[4];
P1←P1+8;
END;
DECOMPOSE(P1,A,LU);
SOLVE(P1,LU,B,SOLN);
IMPROVE(P1,A,LU,B,SOLN,T2);
UNDERFLOW(0);
END;
PRELOAD_WITH 0.5, 0.5, 0.25, 1.0, 1.0, 2.5;
SAFE REAL ARRAY OSHOOT[1:6];
BOOLEAN SIMPLE PROCEDURE OVERSHOOT(REAL A3,A4,DELTA,OFF;REFERENCE REAL T,D);
RETURN( A4 ∧ OFF< (T←-(3*A3)/(4*A4)) <1+OFF ∧(D←(A4*T+A3)*T↑3/ABS(T))
∧ (IF D*DELTA>0 THEN (D←DELTA-D) ELSE (D←-D)));
INTEGER SIMPLE PROCEDURE KISEKI;BEGIN
INTEGER P2S,N,I,J,OVER;
REAL ANG,T;
SIMPLE PROCEDURE BUMP_UP(INTEGER I;REAL ANG,T);
BEGIN INTEGER J;
OVER←OVER+1;
FOR J←MAX_SEG STEP -1 UNTIL I+1 DO BEGIN
POSITION[JOINT,J]←POSITION[JOINT,J-1];
PERIOD[JOINT,J]←PERIOD[JOINT,J-1] END;
POSITION[JOINT,I]←ANG;
PERIOD[JOINT,I]←T*PERIOD[JOINT,I];
IF PERIOD[JOINT,I]<1 THEN PERIOD[JOINT,I]←1;
PERIOD[JOINT,I+1]←PERIOD[JOINT,I+1]-PERIOD[JOINT,I] END;
P2S←PTR2-7;
FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
POLY(N←NS[JOINT]);
OVER←0;
IF OVERSHOOT(SOLN[1],SOLN[2],POS[1]-POS[0],0,T,ANG)
THEN BEGIN POSITION[JOINT,1]←POSITION[JOINT,1]+ANG;OVER←1 END;
IF OVERSHOOT(SOLN[J←(N-1)*3],SOLN[J+1],POS[N-1]-POS[N],-1.0,T,ANG)
THEN BEGIN POSITION[JOINT,N-1]←POSITION[JOINT,N-1]+ANG;OVER←1 END;
IF OVER THEN BEGIN
ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
POLY(NS[JOINT])END;
ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+4) END;
FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,1];
SCHEINMAN(DIAG,TH);
ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,NS[J]-1];
SCHEINMAN(DIAG,TH);
DIAG[7]←DIAG[7] LOR '2000000;
ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,NS[J]];
SCHEINMAN(DIAG,TH);
DIAG[7]←DIAG[7] LOR '2000000;
ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
FOR JOINT←2 STEP 1 UNTIL 5 DO BEGIN
OVER←0;
FOR I←NS[JOINT]-1 STEP -1 UNTIL 2 DO
IF QUADROOT(POSITION[JOINT,I-1],KOE[JOINT,(I*3)-3],
KOE[JOINT,(I*3)-2],KOE[JOINT,(I*3)-1],ANG,T)
THEN BUMP_UP(I,ANG,T);
NS[JOINT]←NS[JOINT]+OVER;
IF OVER THEN BEGIN
ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
POLY(NS[JOINT]);
ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+4) END END;
PTR2←P2S;
PACK_UP;
RETURN(0) END;
SIMPLE MESSAGE PROCEDURE TRAJECTORY(REAL ARRAY T0,TF);
BEGIN
INTEGER MT,I,J,N;
REAL INT,R;
SAFE OWN REAL ARRAY AI,AF,DTH[1:6];
INTEGER TR1,TR2;
WHILE TF[6]-T0[6]>180.0 DO TF[6]←TF[6]-360.0;
WHILE TF[6]-T0[6]≤-180.0 DO TF[6]←TF[6]+360.0;
FLUSHP(150,T0);
LIFTOFF(T0,AI,DEPART_ARM,TR1,T1_ARM);
LIFTOFF(TF,AF,ARRIVE_ARM,TR2,T1_ARM);
IF T1_ARM≤0 ∧ TR2<50 THEN TR2←50;
FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←TF[I]+AF[I]-T0[I]-AI[I];
N←IF T2_ARM>0 THEN T2_ARM ELSE RUNTIME(DTH);
FOR I←1 STEP 1 UNTIL 6 DO BEGIN
POSITION[I,0]←T0[I];
POSITION[I,1]←T0[I]+AI[I];
POSITION[I,2]←TF[I]+AF[I];
POSITION[I,3]←TF[I];
NS[I]←3;
PERIOD[I,1]←TR1;
PERIOD[I,2]←N;
PERIOD[I,3]←TR1 END;
LOOP←0;
DURATION←N;
IF KISEKI THEN USERERR(0,1,"OVERSHOOT");
ARRTRAN(LAST_ARM,TF);
ARRTRAN(LAST_PLANNED_ARM,TF);
ARM_TIME←ARM_TIME+N+TR1*2;
ARRTRAN(DEPART_ARM,ARRIVE_ARM);
FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
T1_ARM←T2_ARM←0;
RESET_CONO;
END;
BOOLEAN SIMPLE PROCEDURE IS_NOT_CLEAR(SAFE REAL ARRAY TRANS,J);
BEGIN SAFE OWN REAL ARRAY IP[1:4];
INTEGER FLAG;
REAL R;
BOOLEAN SIMPLE PROCEDURE NOT_CLEAR(REFERENCE REAL J;SAFE REAL ARRAY V);
BEGIN SAFE OWN REAL ARRAY VT[1:4];
EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY V;REFERENCE REAL T;REAL ARRAY F);
TRANSFORM(VT,J,V);
IF VT[3]<0.375 THEN RETURN(TRUE);
IF (VT[1]-SHOLDER[1])↑2+(VT[2]-SHOLDER[2])↑2<25.0 THEN RETURN(TRUE) ELSE RETURN(FALSE);
END;
IP[1]←IP[3]←0.0;IP[4]←1.0;
FOR R←-2.0,2.0 DO BEGIN IP[2]←R;IF NOT_CLEAR(TRANS[1,1],IP)THEN RETURN(TRUE);END;
J[6]←LAST_ARM[6];
ARM_SOLVE(TRANS,J,FLAG);
IF ¬FLAG THEN RETURN(TRUE);
HANDPOS(J);
IP[2]←0.0;
FOR R←3.0,-3.0 DO BEGIN IP[3]←R;IF NOT_CLEAR(T[SQAR(4)],IP) THEN RETURN(TRUE);END;
IP[3]←-38.0;
IF NOT_CLEAR(T[SQAR(3)],IP) THEN RETURN(TRUE) ELSE RETURN(FALSE);
END;
SIMPLE MESSAGE PROCEDURE MOVE_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
BEGIN SAFE OWN REAL ARRAY J[1:6];
FLAG←0;
IF ARM_EXECUTE ∨ IS_NOT_CLEAR(T,J) THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
FLAG←-1;
TRAJECTORY(LAST_ARM,J);
ARRTRAN(LAST_TRANS,T);
ARRTRAN(LAST_PLANNED_TRANS,T);
END;
SIMPLE MESSAGE PROCEDURE TO_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
BEGIN SAFE OWN REAL ARRAY DTH,J[1:6];
SAFE OWN REAL ARRAY EXF[1:7];
INTEGER K;
FLAG←0;
IF IS_NOT_CLEAR(T,J) THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
FLAG←-1;
IF ARM_EXECUTE THEN BEGIN
FOR K←1 STEP 1 UNTIL 6 DO DTH[K]←J[K]-ARM_VECTOR[K];
ARM_MESSAGE[21]←'17000000;
ARRBLT(ARM_MESSAGE[1],T[1,1],12);
SCHEINMAN(EXF,J);
ARRBLT(ARM_MESSAGE[14],EXF[1],7);
RESET_CONO;
ARM_MESSAGE[13]←IF (K←RUNTIME(DTH))>50 THEN K ELSE 50;
ARMFN(20);
ARM_EXECUTE←FALSE;
UPDATE_SEG;
RETURN END;
FLUSHP(30,LAST_ARM);
PTR3←PTR3+1;
IF MERGE THEN BEGIN
STACK[PTR3]←'417000000+(PTR4←PTR4+1);
STACK[PTR3]↔STACK[PTR3-1];
MERGE←FALSE END ELSE
STACK[PTR3]←'17000000+(PTR4←PTR4+1);
ARRBLT(COEFF[PTR4],T[1,1],12);
COEFF[PTR4←PTR4+12]←T2_ARM;
SCHEINMAN(DIAG,J);
ARRBLT(COEFF[PTR4←PTR4+1],DIAG[1],7);
PTR4←PTR4+6;
ARRTRAN(LAST_ARM,J);
ARM_TIME←ARM_TIME+T2_ARM;
ARRTRAN(LAST_PLANNED_ARM,J);
ARRTRAN(LAST_PLANNED_TRANS,T);
ARRTRAN(LAST_TRANS,T);
ARRTRAN(DEPART_ARM,ARRIVE_ARM);
FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
T1_ARM←T2_ARM←0;
RESET_CONO;
END;
SIMPLE MESSAGE PROCEDURE GO_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
BEGIN SAFE OWN REAL ARRAY TF[1:6];
INTEGER K,TIME,MT,I,J,N,NNULS;
REAL INT,R;
SAFE OWN REAL ARRAY TH,DTH,AI,AF[1:6];
SAFE OWN REAL ARRAY T1,T2[1:4,1:4];
INTEGER TR1,TR2;
FLAG←0;
IF ARM_EXECUTE ∨ IS_NOT_CLEAR(T,TF) THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
FLAG←-1;
FLUSHP(90,LAST_ARM);
PTR3←PTR3+1;
IF MERGE THEN BEGIN
STACK[PTR3]←'420000000+(PTR4←PTR4+1);
STACK[PTR3]↔STACK[PTR3-1];
MERGE←FALSE END ELSE
STACK[PTR3]←'20000000+(PTR4←PTR4+1);
LIFTOFF(LAST_ARM,AI,DEPART_ARM,TR1,T1_ARM);
LIFTOFF(TF,AF,ARRIVE_ARM,TR2,T1_ARM);
IF T1_ARM≤0 ∧ TR2<50 THEN TR2←50;
FOR I←1 STEP 1 UNTIL 6 DO TH[I]←LAST_ARM[I]+AI[I];
INVERT(T1,LAST_TRANS);
MOVET(T2,LAST_TRANS);
FOR I←1 STEP 1 UNTIL 3 DO T2[I,4]←T2[I,4]+DEPART_ARM[I];
TIMES(T1,T1,T2);
ARRBLT(COEFF[PTR4],T1[1,1],12);
COEFF[PTR4←PTR4+12]←TR1;
INVERT(T1,T);
MOVET(T2,T);
FOR I←1 STEP 1 UNTIL 3 DO T2[I,4]←T2[I,4]+ARRIVE_ARM[I];
TIMES(T1,T1,T2);
ARRBLT(COEFF[PTR4←PTR4+1],T1[1,1],12);
COEFF[PTR4←PTR4+12]←T2_ARM;
ARRBLT(COEFF[PTR4←PTR4+1],T[1,1],12);
COEFF[PTR4←PTR4+12]←TR2;
SCHEINMAN(DIAG,TH);
ARRBLT(COEFF[PTR4←PTR4+1],DIAG[1],7);
FOR I←1 STEP 1 UNTIL 6 DO TH[I]←TF[I]+AF[I];
SCHEINMAN(DIAG,TH);
DIAG[7]←DIAG[7] LOR '2000000;
ARRBLT(COEFF[PTR4←PTR4+7],DIAG[1],7);
SCHEINMAN(DIAG,TF);
DIAG[7]←DIAG[7] LOR '2000000;
ARRBLT(COEFF[PTR4←PTR4+7],DIAG[1],7);
PTR4←PTR4+6;
ARRTRAN(LAST_ARM,TF);
ARM_TIME←ARM_TIME+N+TR1+TR2;
ARRTRAN(LAST_PLANNED_ARM,TF);
ARRTRAN(LAST_PLANNED_TRANS,T);
ARRTRAN(LAST_TRANS,T);
ARRTRAN(DEPART_ARM,ARRIVE_ARM);
FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
T1_ARM←T2_ARM←0;
RESET_CONO;
END;
SIMPLE MESSAGE PROCEDURE DRAW_ARM(REAL ARRAY PROFILE;INTEGER STAT);
BEGIN
SAFE OWN REAL ARRAY DTH[1:6];
SAFE OWN REAL ARRAY FAS[0:6,1:6];
SAFE OWN REAL ARRAY ROTS,ROTA,RS,IP,RV,CV,FVV,FV,VT,DIFF[1:4];
REAL AR,RR,THP4,R,MAX;
INTEGER TIME,NF,I,J,K,NT;
EXTERNAL SIMPLE PROCEDURE MOVEV(REAL ARRAY R;REFERENCE REAL A);
IF ARM_EXECUTE THEN BEGIN STAT←4;ARM_EXECUTE←FALSE;RETURN END;
ARRTRAN(TRANS,LAST_TRANS);
CVV(IP,LAST_TRANS,4);
ARRTRAN(FAS,FREE_ARM);
MOVEV(DIFF,PROFILE[1,1]);
MOVEV(RV,PROFILE[2,1]);
IF(AR←IF MAGNITUDE(RV) THEN PROFILE[3,1] ELSE 0) THEN UNIT(RV,RV);
MOVEV(ROTA,PROFILE[5,1]);
IF(RR←IF MAGNITUDE(ROTA) THEN PROFILE[3,2] ELSE 0)
THEN BEGIN UNIT(ROTA,ROTA);
MOVEV(ROTS,PROFILE[4,1]);
DIFFERENCE(IP,IP,ROTS) END;
FVV[1]←FV[1]←FORCE_ARM[1];
FVV[2]←FV[2]←FORCE_ARM[2];
FVV[3]←FV[3]←FORCE_ARM[3];
FVV[4]←FV[4]←1.0;
FLUSH(0,LAST_ARM);
SCHEINMAN(DIAG,LAST_ARM);
ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
FRST_OPEN←TRUE;
R←0;
IF PROFILE[0,3] THEN LOOP←PROFILE[0,3]-1;
IF LOOP THEN NT←8 ELSE BEGIN
NT←4;
R←MAGNITUDE(DIFF);
IF R>15.0 THEN NT←(R/5)+0.5;
NT←IF (MAX←AR/60.0)>NT THEN MAX ELSE NT;
NT←IF (MAX←RR/60.0)>NT THEN MAX ELSE NT END;
IF TYP_HAND THEN OUTSTR(CVS(NT)&" PART TRAJECTORY"&CRLF);
IF NT>8 THEN BEGIN STAT←3;RETURN END;
FOR J←1 STEP 1 UNTIL 6 DO POSITION[J,0]←LAST_ARM[J];
TH[6]←LAST_ARM[6];
FOR I←1 STEP 1 UNTIL NT DO
BEGIN
R←IF I=1
THEN 1/(3*(NT-2))
ELSE (I-1)/(NT-2);
R←IF I=NT-1
THEN R-1/(3*(NT-2))
ELSE IF I=NT
THEN 1.0
ELSE R;
SCALE(VT,DIFF,R);
PLUS(VT,VT,IP);
IF RR THEN BEGIN ROTATE(RS,ROTS,ROTA,R*RR);
PLUS(VT,VT,RS)END;
REDUCE(VT);
CVC(TRANS,4,VT);
IF AR THEN FOR J←1 STEP 1 UNTIL 3 DO BEGIN
CVV(CV,LAST_TRANS,J);
REVOLVE(CV,RV,R*AR);
CVC(TRANS,J,CV) END;
THP4←TH[4];
ARM_SOLVE(TRANS,TH,STAT);
IF ¬STAT THEN BEGIN STAT←1;RETURN END;
IF ABS(THP4-TH[4])>90.0 THEN BEGIN STAT←2;RETURN END;
FOR J←1 STEP 1 UNTIL 6 DO BEGIN
POSITION[J,I]←TH[J];
DTH[I]←POSITION[J,I]-POSITION[J,I-1] END;
IF (K←RUNTIME(DTH))*NT > PROFILE[0,2] THEN PROFILE[0,2]←K*NT;
IF RR THEN BEGIN
FOR K←1 STEP 1 UNTIL FAS[0,1] DO
BEGIN
MOVEV(VT,FAS[K,1]);
VT[4]←1.0;
IF MAGNITUDE(VT)>0.0 THEN
BEGIN
REVOLVE(VT,ROTA,R*RR);
REDUCE(VT);
FOR J←1 STEP 1 UNTIL 3 DO FREE_ARM[K,J]←VT[J] END END;
MOVEV(FV,FVV[1]);
REVOLVE(FV,ROTA,R*RR);
REDUCE(FV);
ARRBLT(FORCE_ARM[1],FV[1],3) END;
SCHEINMAN(DIAG,TH);
ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
END;
TIME←(PROFILE[0,2]/NT)+0.5;
FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
NS[JOINT]←NT;
FOR J←1 STEP 1 UNTIL NT DO PERIOD[JOINT,J]←TIME;
ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
POLY(NS[JOINT]);
ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+6) END;
PTR2←486;
PACK_UP;
DURATION←TIME;
ARRTRAN(LAST_ARM,TH);
ARRTRAN(LAST_TRANS,TRANS);
ARRTRAN(LAST_PLANNED_ARM,TH);
ARRTRAN(LAST_PLANNED_TRANS,TRANS);
ARM_TIME←ARM_TIME+TIME*((NT-2)*LOOP+2);
STAT←0;
RESET_CONO;
FRST_OPEN←FALSE;
END;
SIMPLE MESSAGE PROCEDURE START_TRAJECTORY(STRING FILE;INTEGER START);
BEGIN
STRING S;
LABEL FIND,NONE,FOUND;
REAL R;
FRST_OPEN←TRUE;
ARM_TIME←0;
IF ¬START THEN ARRTRAN(LAST_ARM,ARM_VECTOR);
ARRTRAN(LAST_PLANNED_ARM,LAST_ARM);
WHILE LAST_ARM[6]>180.0 DO LAST_ARM[6]←LAST_ARM[6]-360.0;
WHILE LAST_ARM[6]≤180.0 DO LAST_ARM[6]←LAST_ARM[6]+360.0;
HANDPOS(LAST_ARM);
ARRBLT(LAST_TRANS[1,1],T[SQAR(6)],16);
ARRTRAN(LAST_PLANNED_TRANS,LAST_TRANS);
IF FAST THEN BEGIN
FIND: IF NEXT_BAND>'37 THEN GO TO NONE;
FOR BAND←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[BAND])THEN GO TO FOUND;
TRACK[BAND]←CALL('100000000000 LOR BAND,"UFBGET");
IF ¬_SKIP_ THEN GO TO NONE;
NEXT_BAND←BAND+1;
BANDS[BAND]←FILE;
FOUND: GOODIE[3]←0;
BAND_NUMBER←TRACK[BAND];
END ELSE NONE:BEGIN
FAST←FALSE;
OPEN('16,"DSK",'17,0,0,120,BREAK,EOF);
ENTER('16,FILE&".TRJ",BREAK);
END;
COEFF[0]←0; ARRBLT(COEFF[1],COEFF[0],'1037);
PTR4←PTR3←0;
PTR2←512;
END;
SIMPLE MESSAGE PROCEDURE CLOSE_TRAJECTORY;
FLUSH(1,TH);
SIMPLE MESSAGE PROCEDURE ARM_POSITION;
BEGIN
ARMPOS;
UPDATE_SEG;
END;
SIMPLE MESSAGE PROCEDURE OPEN_HAND(REAL OPENING);
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[1]←'1000000 LOR (OPENING LSH -18);
HANDFN;
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN
FLUSHP(100,LAST_ARM);
STACK[PTR3←PTR3+1]←'1000000 LOR (OPENING LSH -18) END;
SIMPLE MESSAGE PROCEDURE WOBBLE_HAND(REAL WOBBLEING);
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[1]←'14000000 LOR (WOBBLEING LSH -18);
HANDFN;
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN FLUSHP(1,LAST_ARM);STACK[PTR3←PTR3+1]←'14000000 LOR (WOBBLEING LSH -18) END;
SIMPLE MESSAGE PROCEDURE PLACE_ARM;
BEGIN SAFE OWN REAL ARRAY DIR[1:4],DTH[1:6];
INTEGER I;
FOR I←1 STEP 1 UNTIL 3 DO DIR[I]←IF I=3 THEN -0.03 ELSE 0;
DIR[4]←1.0;
INCREMENT(DTH,DIR,FALSE);
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[7]←'4000000;
ARRBLT(ARM_MESSAGE[1],DTH[1],6);
ARMFN(6);
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN FLUSHP(7,LAST_ARM);
STACK[PTR3←PTR3+1]←'4000000+(PTR4←PTR4+1);
ARRBLT(COEFF[PTR4],DTH[1],6);
PTR4←PTR4+5 END;
END;
SIMPLE MESSAGE PROCEDURE SEARCH_ARM(REAL SIZE;REAL ARRAY N,F);
BEGIN SAFE OWN REAL ARRAY SEC[1:4];
IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
CROSS(SEC,N,F);
UNIT(F,F);
UNIT(SEC,SEC);
SCALE(F,F,SIZE);
SCALE(SEC,SEC,SIZE);
REDUCE(F);
REDUCE(SEC);
FLUSHP(40,LAST_ARM);
STACK[PTR3←PTR3+1]←'15000000+(PTR4←PTR4+1);
ARRBLT(COEFF[PTR4],F[1],3);
ARRBLT(COEFF[PTR4←PTR4+3],SEC[1],3);
ARRBLT(COEFF[PTR4←PTR4+3],LAST_PLANNED_TRANS[1,1],12);
ARRBLT(COEFF[PTR4←PTR4+12],LAST_PLANNED_ARM[1],6);
PTR4←PTR4+6;
END;
SIMPLE MESSAGE PROCEDURE PARK_ARM;
BEGIN INTEGER K;
IF ¬ARM_EXECUTE THEN
BEGIN
ARRIVE_ARM[3]←0.0;
GO_ARM(PARK_TRANS,K);
END ELSE TO_ARM(PARK_TRANS,K);
END;
SIMPLE MESSAGE PROCEDURE WAIT_ARM;
IF ¬ARM_EXECUTE THEN BEGIN
FLUSHP(1,LAST_ARM);
STACK[PTR3←PTR3+1]←'3000000 END ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE CLOSE_HAND(REAL DIST);
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[1]←'2000000 LOR (DIST LSH -18);
HANDFN;
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN FLUSHP(100,LAST_ARM);STACK[PTR3←PTR3+1]←'2000000 LOR (DIST LSH -18)END;
SIMPLE MESSAGE PROCEDURE CHANGE_ARM(REAL ARRAY DIR;REAL DIST;
REAL ARRAY AXIS;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
BEGIN INTEGER J,N;
SAFE OWN REAL ARRAY DTH,AV[1:6];
SAFE OWN REAL ARRAY EXF[1:7];
SAFE OWN REAL ARRAY DT,TT[1:4,1:4];
SAFE OWN REAL ARRAY VT[1:4];
IF ARM_EXECUTE
THEN ARRBLT(TT[1,1],ARM_LINK[6,1,1],16)
ELSE ARRTRAN(TT,LAST_TRANS);
SCALE(VT,DIR,DIST);
REDUCE(VT);
ARRTRAN(DT,TT);
FOR J←1 STEP 1 UNTIL 3 DO DT[J,4]←DT[J,4]+VT[J];
IF DEG ∧ MAGNITUDE(AXIS) THEN BEGIN
UNIT(AXIS,AXIS);
FOR I←1 STEP 1 UNTIL 3 DO BEGIN
CVV(VT,DT,I);
REVOLVE(VT,AXIS,DEG);
CVC(DT,I,VT) END;
END;
INVERT(TT,TT);
TIMES(TT,TT,DT);
ARM_SOLVE(DT,AV,FLAG);
FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←AV[I]-(IF AEF THEN ARM_VECTOR[I] ELSE LAST_ARM[I]);
IF ¬FLAG THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[21]←'5000000;
ARRBLT(ARM_MESSAGE[1],TT[1,1],12) END
ELSE BEGIN ARRTRAN(LAST_ARM,AV);
FLUSHP(70,DT);
PTR3←PTR3+1;
IF MERGE THEN BEGIN
STACK[PTR3]←'405000000+(PTR4←PTR4+1);
STACK[PTR3]↔STACK[PTR3-1];
MERGE←FALSE END ELSE
STACK[PTR3]←'5000000+(PTR4←PTR4+1);
ARRBLT(COEFF[PTR4],TT[1,1],12) END;
SCHEINMAN(EXF,AV);
IF ARM_EXECUTE
THEN ARRBLT(ARM_MESSAGE[13],EXF[1],7)
ELSE BEGIN ARRBLT(COEFF[PTR4←PTR4+12],EXF[1],7);
PTR4←PTR4+6 END;
RESET_CONO;
IF TIME≤0 THEN TIME←IF (J←RUNTIME(DTH))>50 THEN J ELSE 50;
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[20]←TIME;
ARMFN(20);
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN COEFF[PTR4←PTR4+1]←TIME;
ARM_TIME←ARM_TIME+TIME;
ARRTRAN(LAST_TRANS,DT) END;
END;
SIMPLE MESSAGE PROCEDURE CENTER_HAND(REAL DIST);
BEGIN INTEGER J,N;
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[2]←'12000000;
ARRBLT(ARM_MESSAGE[1],DIST,1);
ARMFN(1);
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN FLUSHP(24,LAST_ARM);
STACK[PTR3←PTR3+1]←'12000000+(PTR4←PTR4+1);
ARRBLT(COEFF[PTR4],DIST,1) END;
END;
SIMPLE MESSAGE PROCEDURE SET_ARM(STRING ST;REAL ARRAY V,WRT);
BEGIN
SAFE OWN REAL ARRAY T[1:4,1:4];
INVERT(T,WRT);
TIMES(T,V,T);
IF ARM_EXECUTE THEN BEGIN
ARM_MESSAGE[14]←'13000000;
ARM_MESSAGE[1]←CVSIX(ST);
ARRBLT(ARM_MESSAGE[2],T[1,1],12);
ARMFN(13);
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN
FLUSHP(4,LAST_ARM);
STACK[PTR3←PTR3+1]←'13000000+(PTR4←PTR4+1);
COEFF[PTR4]←CVSIX(ST);
ARRBLT(COEFF[PTR4←PTR4+1],T[1,1],12);
PTR4←PTR4+11 END;
END"SET_ARM";
SIMPLE MESSAGE PROCEDURE DRIVE_ARM(INTEGER JOINT;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
BEGIN INTEGER I,J,N;
REAL R;
SAFE OWN REAL ARRAY EXF[1:7];
SAFE OWN REAL ARRAY LA[1:6];
FLAG←FALSE;
IF ARM_EXECUTE THEN ARRBLT(LA[1],ARM_VECTOR[1],6) ELSE ARRTRAN(LA,LAST_ARM);
R←LA[JOINT]+DEG;
IF JOINT<6 ∧ (STOP[JOINT,1]-R)*(R-STOP[JOINT,2])<1 THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
FLAG←TRUE;
LA[JOINT]←R;
SCHEINMAN(EXF,LA);
IF ARM_EXECUTE
THEN BEGIN ARM_MESSAGE[15]←'5000000;
ARM_MESSAGE[1]←0;ARRBLT(ARM_MESSAGE[2],ARM_MESSAGE[1],5);
ARRBLT(ARM_MESSAGE[JOINT],DEG,1) END
ELSE BEGIN ARRBLT(LAST_TRANS[1,1],T[85],16);
FLUSHP(50,LAST_ARM);
PTR3←PTR3+1;
IF MERGE THEN BEGIN
STACK[PTR3]←'405000000+(PTR4←PTR4+1);
STACK[PTR3]↔STACK[PTR3-1];
MERGE←FALSE END ELSE
STACK[PTR3]←'5000000+(PTR4←PTR4+1);
COEFF[PTR4]←0;
ARRBLT(COEFF[PTR4+1],COEFF[PTR4],5);
ARRBLT(COEFF[PTR4+JOINT-1],DEG,1) END;
N←IF TIME≤0 THEN (IF JOINT<3 THEN 50 ELSE 20)+ABS(DEG)*TIMFAC[JOINT] ELSE TIME;
IF ARM_EXECUTE
THEN BEGIN ARRBLT(ARM_MESSAGE[7],EXF[1],7);
ARM_MESSAGE[13]←GO_WORD[JOINT]+2;
IF FREE_ARM[0,1] THEN FREE_JOINT(ARM_MESSAGE[13]);
ARM_MESSAGE[14]←N;
ARMFN(14);
ARM_EXECUTE←FALSE;
UPDATE_SEG END
ELSE BEGIN ARRBLT(COEFF[PTR4←PTR4+6],EXF[1],7);
PTR4←PTR4+6;
COEFF[PTR4]←GO_WORD[JOINT]+2;
IF NNUL THEN COEFF[PTR4]←COEFF[PTR4]+'1000000;
IF FREE_ARM[0,1] THEN FREE_JOINT(COEFF[PTR4]);
COEFF[PTR4←PTR4+1]←N;
ARM_TIME←ARM_TIME+N;
ARRTRAN(LAST_ARM,LA) END;
RESET_CONO;
END;
SIMPLE MESSAGE PROCEDURE STOP_ARM(REAL ARRAY F,M);
BEGIN SAFE OWN REAL ARRAY TQ,XF[1:6];
INTEGER I;
REAL FAC,R,MAX;
HANDPOS(LAST_ARM);
REDUCE(F);
ARRBLT(XF[1],F[1],3);
REDUCE(M);
ARRBLT(XF[4],M[1],3);
FORCE(TQ,XF);
MAX←0.0;
FAC←1.5;
FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(TQ[I]/F0[I]))>MAX THEN BEGIN
MAX←R;
BFJ←I END;
R←FAC/MAX;
IF R>1.0 THEN FOR I←1 STEP 1 UNTIL 6 DO TQ[I]←TQ[I]*R;
FLUSHP(7,LAST_ARM);
STACK[PTR3←PTR3+1]←'7000000+(PTR4←PTR4+1);
ARRBLT(COEFF[PTR4],TQ[1],6);
PTR4←PTR4+5;
END;
SIMPLE MESSAGE PROCEDURE NO_NULL;NNUL←-1;
SIMPLE MESSAGE PROCEDURE DO_IT(STRING FILE);
BEGIN INTEGER I;
ARM_STATUS←0;
ARM_EXECUTE←FALSE;
FOR I←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[I])THEN DONE;
DOIT(TRACK[I],CVSIX(FILE));
IF ARM_STATUS='50 ∧TRACK[I] THEN BANDS[I]←NULL;
UPDATE_SEG;
END;
SIMPLE MESSAGE PROCEDURE MERGE_ARM;MERGE←TRUE;
SIMPLE MESSAGE PROCEDURE DO_PROCEED(INTEGER SKIP);
BEGIN
ARM_EXECUTE←FALSE;
IF ¬ARM_WAIT THEN RETURN;
ARMPROCEED(SKIP);
UPDATE_SEG;
END;
SIMPLE MESSAGE PROCEDURE ARM_CONO(REAL ARRAY APPROACH,OBJECT;INTEGER ARRIVE_TIME,DEPART_TIME);
BEGIN ARRTRAN(ARRIVE_ARM,APPROACH);
OBJECT_MASS←OBJECT[4];
OBJECT_KXX←OBJECT[1];
OBJECT_KYY←OBJECT[2];
OBJECT_KZZ←OBJECT[3];
T2_ARM←DEPART_TIME;
T1_ARM←ARRIVE_TIME;
END;
SIMPLE MESSAGE PROCEDURE SET_TOUCH(BOOLEAN STOP_ON_TOUCH);
IF ¬ARM_EXECUTE THEN BEGIN FLUSHP(1,LAST_ARM);
STACK[PTR3←PTR3+1]←'6000000;
IF STOP_ON_TOUCH THEN STACK[PTR3]←STACK[PTR3] LOR '777777;
END ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_SKIPE(INTEGER I);
IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'101000000 LOR I ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_SKIPN(INTEGER I);
IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'103000000 LOR I ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_SKIPS(INTEGER I);
IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'104000000 LOR I ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_JMP(INTEGER I);
IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'102000000 LOR ('777777 LAND I) ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_AOJ(INTEGER I);
IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'16000000 LOR ('777777 LAND I) ELSE ARM_EXECUTE←FALSE;
SIMPLE MESSAGE PROCEDURE ARM_SAVE(STRING S);
BEGIN
SAFE OWN REAL ARRAY T[1:4,1:4];
INVERT(T,LAST_PLANNED_TRANS);
IF ARM_EXECUTE THEN BEGIN ARM_MESSAGE[13]←'10000000;
ARM_MESSAGE[1]←CVSIX(S) END ELSE
BEGIN FLUSHP(20,LAST_ARM);STACK[PTR3←PTR3+1]←'10000000+(PTR4←PTR4+1);
COEFF[PTR4]←CVSIX(S) END;
IF ARM_EXECUTE THEN ARRBLT(ARM_MESSAGE[2],T[1,1],12)
ELSE ARRBLT(COEFF[PTR4←PTR4+1],T[1,1],12);
IF ARM_EXECUTE THEN BEGIN ARMFN(13);
ARM_EXECUTE←FALSE;
UPDATE_SEG END ELSE PTR4←PTR4+12;
END;
SIMPLE MESSAGE PROCEDURE ARM_RESTORE(STRING S;INTEGER FINISH);
BEGIN
LABEL L1;
INTEGER I,J;
SAFE OWN REAL ARRAY T[1:4,1:4];
IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
FOR I←PTR3 STEP -1 UNTIL 1 DO BEGIN
J←STACK[I] LAND '77000000;
IF '17000000 ≤ J ≤'21000000 THEN BEGIN
FOR J←PTR3 STEP -1 UNTIL I DO BEGIN
STACK[J+1]←STACK[J];
END;
PTR3←PTR3+1;
GO TO L1
END;
END;
RETURN;
L1: STACK[I]←'11000000+(PTR4←PTR4+1);
COEFF[PTR4]←CVSIX(S);
COEFF[PTR4←PTR4+1]←FINISH;
ARRBLT(COEFF[PTR4←PTR4+1],LAST_PLANNED_TRANS[1,1],12);
ARRBLT(COEFF[PTR4←PTR4+12],LAST_PLANNED_ARM[1],6);
PTR4←PTR4+6;
END;
SIMPLE MESSAGE PROCEDURE SCREW(REAL VELOCITY);
BEGIN INTEGER I;
IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
STACK[PTR3←PTR3+1]←'22000000 LOR ((I←VELOCITY*1.0) LAND '777777) END;